﻿using System;
using System.Collections.Generic;
using System.Text;
using G3.State;
using System.IO;
using dnAnalytics.LinearAlgebra;

namespace G3.AI
{
    #region HybridDriver
    public class HybridDriver : IDriver
    {
        Matrix weights = MatrixBuilder.CreateMatrix(2, 1 + CONTROLPOINTS,0.5);
        Vector lastWheelInput = VectorBuilder.CreateVector(1 + CONTROLPOINTS);
        Vector lastThrottleInput = VectorBuilder.CreateVector(1 + CONTROLPOINTS);
        Pair lastResponsePair;
        double learningFactor = double.Parse(Controller.Parameters.GetParameter("HybridDriverLearningFactor"));
        const double time = 500;
        //int lastDirection = 1;        
        StreamWriter logFile;
        const int CONTROLPOINTS = 7;
        const int BRAKETRESHOLD = 30;

        public HybridDriver()
        {
            logFile = new StreamWriter("log.txt");
        }

        private int WheelSign(Vector cp)
        {            
            double[] w = { 7.0, 6.0, 5.0, 4.0, 3.0, 2.0, 1.0 };
            Vector a = VectorBuilder.CreateVector(w);
            return Math.Sign(cp.DotProduct(a));
        }

        public Pair Think()
        {
            //State.State currentState = Controller.Controller.GetOnlyInstance().AttachedState;
            Vector cp = Controller.Map.GetControlPoints(Controller.Map.StateCoordsToMapCoords(Controller.Controller.GetOnlyInstance().AttachedState.CarPosition), Controller.Controller.GetOnlyInstance().AttachedState.CarOrientationRadian, CONTROLPOINTS);
            
            

            //preparing input vector for throttle

            lastThrottleInput[0] = Controller.Controller.GetOnlyInstance().AttachedState.CarSpeed;            
            for (int i = 0; i != cp.Count;++i)            
            {
                if (cp[i] == 0)
                {                    
                    lastThrottleInput[i + 1] = 5;
                }
                else
                {                    
                    lastThrottleInput[i + 1] = Math.Abs(3/cp[i]);
                }
            }

            //preparing input vector for wheel
            //lastDirection = WheelSign(cp);
//            currentState = Controller.Controller.GetOnlyInstance().AttachedState;
            lastWheelInput[0] = Controller.Controller.GetOnlyInstance().AttachedState.CarSpeed;
            for (int i = 0; i != cp.Count; ++i)
            {
                lastWheelInput[i + 1] = cp[i];                
            }

            //calculating response vector components            
            Vector response  = VectorBuilder.CreateVector(2);
            
            //wheel
            response[0] = weights.Row(0).DotProduct(lastWheelInput);

            //throttle
            response[1] = weights.Row(1).DotProduct(lastThrottleInput) - BRAKETRESHOLD;
            
            //weights.Multiply(lastInput,response);

            Controller.Controller.GetOnlyInstance().SetWheelThrootlePositions(response[1], response[0]);                                    
            return lastResponsePair = new Pair(response[0], response[1]);
        }

        private void InitLearning()
        {            
            Controller.Controller.GetOnlyInstance().Reset();
            //State.State currentState = Controller.Controller.GetOnlyInstance().AttachedState;
            for (int i = 0; i != 3; ++i)
            {
                Think();
                Controller.Controller.GetOnlyInstance().AttachedState.Simulate(time);
//                currentState = Controller.Controller.GetOnlyInstance().AttachedState;
            }
        }

        public void Teach(int treshold)
        {            
            InitLearning();
            //State.State currentState = Controller.Controller.GetOnlyInstance().AttachedState;

            for (int iteration = 0; iteration != treshold; ++iteration)
            {
                Think();
                //currentState = Controller.Controller.GetOnlyInstance().AttachedState;
                Controller.Controller.GetOnlyInstance().AttachedState.Simulate(time);
                State.State.StateVars source = Controller.Controller.GetOnlyInstance().AttachedState.GetStateVars(-1);
                                
                //we aren't on the road, so we should start from the beginning
                //currentState = Controller.Controller.GetOnlyInstance().AttachedState;
                Pair mapCoords = Controller.Map.StateCoordsToMapCoords(Controller.Controller.GetOnlyInstance().AttachedState.GetStateVars(0).carPosition);
                if (Controller.Map.Fields[(int)Math.Truncate(mapCoords.x), (int)Math.Truncate(mapCoords.y)] != 1)
                {
                    iteration /= 2;
                    InitLearning();
                    Think();
                    //currentState = Controller.Controller.GetOnlyInstance().AttachedState;
                    Controller.Controller.GetOnlyInstance().AttachedState.Simulate(time);
                }

                Pair optimalPosition = Controller.Map.MapCoordsToStateCoords(Controller.Map.GetOptimalPosition());                
                State.State.StateVars destination = new State.State.StateVars(optimalPosition, new Pair(0, 0), lastWheelInput[0]);
                
                Pair optimalResponsePair;
                //currentState = Controller.Controller.GetOnlyInstance().AttachedState;
                if (source.carVelocity.Abs() != 0) optimalResponsePair = Controller.Controller.GetOnlyInstance().AttachedState.ArcSim(source, destination, time);
                else
                {
                    optimalResponsePair.x = lastResponsePair.x;
                    optimalResponsePair.y = 20;
                }

                if (Math.Abs(optimalResponsePair.x) > 100) optimalResponsePair.x = 100 * Math.Sign(optimalResponsePair.x);
                if (Math.Abs(optimalResponsePair.y) > 100) optimalResponsePair.y = 100 * Math.Sign(optimalResponsePair.y);
                if (Math.Abs(lastResponsePair.x) > 100) lastResponsePair.x = 100 * Math.Sign(lastResponsePair.x);
                if (Math.Abs(lastResponsePair.y) > 100) lastResponsePair.y = 100 * Math.Sign(lastResponsePair.y);

                Vector optimalResponse = VectorBuilder.CreateVector(2);
                optimalResponse[0] = optimalResponsePair.x;
                optimalResponse[1] = optimalResponsePair.y;
                Vector lastResponse = VectorBuilder.CreateVector(2);
                lastResponse[0] = lastResponsePair.x;
                lastResponse[1] = lastResponsePair.y;

                double wheelError = optimalResponse[0] - lastResponse[0];
                double throttleError = optimalResponse[1] - lastResponse[1];
                for (int i = 0; i != weights.Columns; ++i)
                {
                    weights[0, i] += learningFactor * lastWheelInput[i] * wheelError;
                    weights[1, i] += learningFactor * lastThrottleInput[i] * throttleError;                    
                }

                logFile.WriteLine("\n\n"+iteration.ToString());
                logFile.WriteLine(weights);
            }
        }

        /// <summary>
        /// Loads the 'knowledge' from the specified file.
        /// </summary>
        /// <param name="fileName"></param>
        public void LoadProfile(String fileName)
        {
            if (fileName == "") return;
            StreamReader input = new StreamReader(fileName);
            int i = 0;
            char[] separator = { '\t' };
            while (!input.EndOfStream)
            {
                String line = input.ReadLine();
                if (line != "")
                {
                    String[] parts = line.Split(separator,StringSplitOptions.RemoveEmptyEntries);
                    for(int j = 0; j!=parts.Length; ++j)
                    {
                        weights[i, j] = double.Parse(parts[j]);
                    }
                }
                ++i;
            }
            input.Close();
        }

        /// <summary>
        /// Save the current 'knowledge' to file.
        /// </summary>
        /// <param name="fileName"></param>
        public void SaveProfile(String fileName)
        {
            if (fileName == "") return;
            StreamWriter output = new StreamWriter(fileName);            
            for (int i = 0; i != weights.Rows;++i )
            {
                for (int j = 0; j != weights.Columns; ++j)
                {
                    output.Write(weights[i, j].ToString()+'\t');
                }
                output.Write('\n');
            }
            output.Close();
        }

        public DriverType Type()
        {
            return DriverType.Hybrid;
        }

    }
    #endregion
}
